#include "carobject.h"
#include <math.h>
#include <QDebug>

CarObject::CarObject()
{
    hardInit();
    connect(&mpu6050Timer,SIGNAL(timeout()),this,SLOT(onMpu6050Update()));
    mpu6050Timer.start(50);
}

void CarObject::hardInit()
{
    wiringPiSetup();
    //--- 初始化 电机 控制 ---
    pinMode(IO_EN_L, INPUT);
    pinMode(IO_EN_R, INPUT);
    digitalWrite(IO_EN_L,1);
    digitalWrite(IO_EN_R,1);

    softPwmCreate(IO_PWM_R_1,0,100);
    softPwmCreate(IO_PWM_R_2,0,100);
    softPwmCreate(IO_PWM_L_1,0,100);
    softPwmCreate(IO_PWM_L_2,0,100);
    softPwmWrite(IO_PWM_R_1,0);
    softPwmWrite(IO_PWM_R_2,0);
    softPwmWrite(IO_PWM_L_1,0);
    softPwmWrite(IO_PWM_L_2,0);

    //--- 初始化 舵机 控制 ---
    pinMode(IO_SERVO_H,PWM_OUTPUT);
    pinMode(IO_SERVO_V,PWM_OUTPUT);
    pwmSetMode (PWM_MODE_MS);
    pwmSetRange (2000);
    pwmSetClock (192);
    pwmWrite(IO_SERVO_H, 150);  //50-250   up    - down
    pwmWrite(IO_SERVO_V, 150);  //50-250   right - left

    //--- 初始化 direct 舵机 控制 ---
//    softPwmCreate(IO_PWM_L_2,0,2000);
//    softPwmWrite(IO_PWM_L_2,250);
}

void CarObject::setPwmMotor_L(int pwm)
{
    if(pwm > 0)
    {
        softPwmWrite(IO_PWM_R_1,abs(pwm));
        softPwmWrite(IO_PWM_R_2,0);
    }
    else
    {
        softPwmWrite(IO_PWM_R_1,0);
        softPwmWrite(IO_PWM_R_2,abs(pwm));
    }
    mData.pwmMotorL = pwm;
    qDebug()<<"Motor_1 Out PWM:"<<pwm;
}

void CarObject::setPwmMotor_R(int pwm)
{
    if(pwm > 0)
    {
        softPwmWrite(IO_PWM_L_1,abs(pwm));
        softPwmWrite(IO_PWM_L_2,0);
    }
    else
    {
        softPwmWrite(IO_PWM_L_1,0);
        softPwmWrite(IO_PWM_L_2,abs(pwm));
    }
    mData.pwmMotorR = pwm;
    qDebug()<<"Motor_2 Out PWM:"<<pwm;
}

CarData CarObject::getData()
{
    return mData;
}

void CarObject::setAngleServo_V(int angle)  //-90 - 90   50-250   right - left
{
    //-90-90 ---> -100-100  ---> 50-250
    int duty = angle * 10 / 9;
    duty += 150;
    pwmWrite(IO_SERVO_V,duty);
    mData.angleServoV = angle;
    qDebug()<<"set Servo_V Angle :"<<angle;
}
void CarObject::setAngleServo_H(int angle)  //-90 - 90   50-250   up    - down
{
    //-90-90 ---> -100-100  ---> 50-250
    int duty = angle * 10 / 9;
    duty += 150;
    pwmWrite(IO_SERVO_H,duty);
    mData.angleServoH = angle;
    qDebug()<<"set Servo_H Angle :"<<angle;
}

void CarObject::onMpu6050Update()
{
    mData.acc_X = mMpu6050.accX();
    mData.acc_Y = mMpu6050.accY();
    mData.acc_Z = mMpu6050.accZ();

    mData.gyro_X = mMpu6050.gyroX();
    mData.gyro_Y = mMpu6050.gyroY();
    mData.gyro_Z = mMpu6050.gyroZ();
}


